clear all
close all
clc
s = serial('COM6');
%set(s,'BaudRate',38400);
set(s,'BaudRate',115200);
set(s,'DataBits',8);
set(s,'StopBits',1);
fopen(s);

out2 = [];
NB_SAMPLE = 50000;

t = 0;
bon = 0;
while(bon == 0)
    out = fread(s, 1);
    if (out == 10)
        out = fread(s, 12);
        bon = 1;
    end
end

tic;
for i = 1 : NB_SAMPLE
    out = fread(s, 2);
    if (out(1) == 13 && out(2) == 10)
        temp = fread(s, 12);
        out2 = [out2 (temp(1)+temp(2)*256) (temp(3)+temp(4)*256) (temp(5)+temp(6)*256) (temp(7)+temp(8)*256) (temp(9)+temp(10)*256) (temp(11)+temp(12)*256)];
    end
    
    if (mod(i, 300) == 0)
        i
        old_t = t;
        t = toc;
        sps = 300 / (t-old_t)
        %     p = (out2(1:6:end) - 32768 )' ;
        %     plot(p); hold on;
        %     q = (out2(2:6:end) - 32768 )';
        %     plot(q,'r');hold on;
        %     r = (out2(3:6:end) - 32768 )';
        %     plot(r,'g');
        %
        %    xacc = (out2(4:6:end) - 32768 )';
        %    plot(xacc); hold on;
        %    yacc = (out2(5:6:end) - 32768 )';
        %    plot(yacc,'r');hold on;
        %    zacc = (out2(6:6:end) - 32768 )';
        %    plot(zacc,'g');
        %
        %    drawnow
    end
    
end
sps = NB_SAMPLE / toc
out2 = out2 - 32768;

fclose(s);

csvwrite('adc raw values.txt', out2);

figure;
p = out2(1:6:end)';
plot(p - mean(p)); hold on;
q = out2(2:6:end)';
plot(q - mean(q),'r');hold on;
r = out2(3:6:end)';
plot(r - mean(r),'g');
title('Gyros');
legend('X', 'Y', 'Z');

figure;
xacc = out2(4:6:end)';
plot(xacc - mean(xacc)); hold on;
yacc = out2(5:6:end)';
plot(yacc - mean(yacc),'r');hold on;
zacc = out2(6:6:end)';
plot(zacc - mean(zacc),'g');
title('Acceleros');
legend('X', 'Y', 'Z');

p4 = bitshift(p, -4);
q4 = bitshift(q, -4);
r4 = bitshift(r, -4);
xacc4 = bitshift(xacc, -4);
yacc4 = bitshift(yacc, -4);
zacc4 = bitshift(zacc, -4);
%
% figure;
% plot(p4); hold on;
% plot(q4,'r');hold on;
% plot(r4,'g');
% title('Gyros with 4 right shifting');
% legend('X', 'Y', 'Z');
%
% figure;
% plot(xacc4); hold on;
% plot(yacc4,'r');hold on;
% plot(zacc4,'g');
% title('Acceleros with 4 right shifting');
% legend('X', 'Y', 'Z');

debut = 1;
fin = 1775;
[var(p(debut:fin))
    var(q(debut:fin))
    var(r(debut:fin))
    var(xacc(debut:fin))
    var(yacc(debut:fin))
    var(zacc(debut:fin))];

[var(p4(debut:fin))
    var(q4(debut:fin))
    var(r4(debut:fin))
    var(xacc4(debut:fin))
    var(yacc4(debut:fin))
    var(zacc4(debut:fin))]*256;

% xaccmoy = csaps(1:1:length(xacc),xacc')
% yaccmoy = csaps(1:1:length(xacc),xacc')
% zaccmoy = csaps(1:1:length(xacc),xacc')
% fnplt(xaccmoy)
%     figure;
%     plot(1:1:length(xacc),fnplt(xaccmoy)); hold on;
%     plot(yaccmoy,'r');hold on;
%     plot(zaccmoy,'g');
%     title('Acceleros with 4 right shifting');
%     legend('X', 'Y', 'Z');

